#include "tf2_node.hpp"  

TF2Node::TF2Node()
{   
    // 1. 根据欧拉角初始化化tf2::Quaternion
    q_tf_.setRPY(-1.57, 0, -1.57);    // x-y-z 这里第一个参数为roll， 与eigen初始化相反
    
    Eigen::Vector3d euler(-1.57, 0, 1.57);   // z-y-x
    // q_eigen_ = Eigen::Quaterniond(euler);
    // 2. 根据欧拉角初始化化eigen::Quaterniond 注意欧拉角参数的顺序
    q_eigen_ = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitZ()) * 
			  Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) * 
			  Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitX());

    std::cout << q_tf_.x() << "\t" 
        << q_tf_.y() << "\t" 
        << q_tf_.z() << "\t" 
        << q_tf_.w() << std::endl;

    std::cout << q_eigen_.coeffs().transpose() << std::endl; 

    Eigen::Vector3d euler1 = Eigen::Vector3d::Identity();
    Eigen::Vector3d euler2 = Eigen::Vector3d::Identity();
    // 3. tf2获得欧拉角 注意顺序x-y-z
    tf2::Matrix3x3(q_tf_).getRPY(euler1(2), euler1(1), euler1(0));  // 欧拉角顺序注意索引2放第一个位置的
    
    // 4. eigen获得欧拉角 注意顺序z-y-x
    euler2 = q_eigen_.toRotationMatrix().eulerAngles(2, 1, 0);

    std::cout << "\n-----\ntf2(rpy) 欧拉角顺序(2,1,0): " << euler1.transpose() << std::endl; 
    std::cout << "eigen 欧拉角顺序(2,1,0): " << euler2.transpose() << std::endl; 

    static_broadcaster_.reset(new tf2_ros::StaticTransformBroadcaster()); 
    broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>();
    pubTransforms();

    pub_path_ = n_.advertise<nav_msgs::Path>("/path",10);
    msg_path_.reset(new nav_msgs::Path());

    sub_rgb_ = n_.subscribe("/camera/color/image_raw", 10, &TF2Node::callback, this);
     
    // --------------------
    first_imu_ = true;
    first_flag = true;
    a0_ = Eigen::Vector3d::Zero();
    w0_ = Eigen::Vector3d::Zero();

    euler_a_ = Eigen::Vector3d::Zero();
    euler_w_ = Eigen::Vector3d::Zero();
    euler_w0_ = Eigen::Vector3d::Zero();
    
    // q_a_ = Eigen::Quaterniond::Identity();
    q_w_ = Eigen::Quaterniond::Identity();
    q0_ = Eigen::Quaterniond::Identity();

    sub_imu_ = n_.subscribe("/imu/data", 1000, &TF2Node::callbackImu, this);

}

// ------------发布一个静态坐标系-----------------
void TF2Node::pubTransforms()
{   
    geometry_msgs::TransformStamped tf1;
    tf1.header.stamp = ros::Time::now();
    tf1.child_frame_id = "camera";
    tf1.header.frame_id = "map";

    tf2::Quaternion q_init;
    q_init.setRPY(-1.57, 0, -1.57);
    tf1.transform.rotation.x = q_init.x();
    tf1.transform.rotation.y = q_init.y();
    tf1.transform.rotation.z = q_init.z();
    tf1.transform.rotation.w = q_init.w();
    tf1.transform.translation.x = 0.3;
    tf1.transform.translation.y = 0.5;
    tf1.transform.translation.z = -0.1;

    static_broadcaster_->sendTransform(tf1);
}

void TF2Node::callback(const sensor_msgs::ImageConstPtr & msg_img)
{
    ROS_INFO("Publish tf successful, index = %s", std::to_string(i).c_str());
    tf_.header.stamp = ros::Time::now();
    tf_.child_frame_id = "camera_1";
    tf_.header.frame_id = "map";

    q_tf_.normalize();

    tf_.transform.rotation.x = q_tf_.x();
    tf_.transform.rotation.y = q_tf_.y();
    tf_.transform.rotation.z = q_tf_.z();
    tf_.transform.rotation.w = q_tf_.w();

    Eigen::AngleAxisd rotation_vector(1 / 180. * M_PI, Eigen::Vector3d(0, 0, 1));
    Eigen::Vector3d euler = rotation_vector.toRotationMatrix().eulerAngles(2, 1, 0);
    // Eigen::Vector3d euler1 = euler * 180 / M_PI; 
    // std::cout << euler1 << std::endl;
    
    tf2::Quaternion delta_q;
    delta_q.setRPY(euler[2], euler[1], euler[0]);

    q_tf_ = delta_q * q_tf_;
    // x(t)=a(2cost-cos2t)
    // y(t)=a(2sint-sin2t)
    double t = i * M_PI / 360;
    
    // 心型线
    tf_.transform.translation.x = 0.5 * (2 * cos(t) - cos(2 * t));    
    tf_.transform.translation.y = 0.5 * (2 * sin(t) - sin(2 * t));
    tf_.transform.translation.z = 0.001 * i / 2;
    
    i++;
    broadcaster_->sendTransform(tf_);

    geometry_msgs::PoseStamped::Ptr pose(new geometry_msgs::PoseStamped());
    
    pose->header.frame_id = "map";
    pose->header.stamp = ros::Time::now();

    pose->pose.position.x = tf_.transform.translation.x;
    pose->pose.position.y = tf_.transform.translation.y;
    pose->pose.position.z = tf_.transform.translation.z;

    pose->pose.orientation.x = tf_.transform.rotation.x;
    pose->pose.orientation.y = tf_.transform.rotation.y;
    pose->pose.orientation.z = tf_.transform.rotation.z;
    pose->pose.orientation.w = tf_.transform.rotation.w;
    
    msg_path_->header.stamp = ros::Time::now();
    msg_path_->header.frame_id = "map";
    msg_path_->poses.emplace_back(*pose);

    pub_path_.publish(msg_path_);
}

void TF2Node::callbackImu(const sensor_msgs::ImuConstPtr &msg_imu)
{   
    // imu系下的测量值  静止时a=(0, 0, -9.8)   // 普通姿态解算,不用去重力,直接用测量值
    // Eigen::Vector3d gn(0, 0, -9.8);
    Eigen::Vector3d a(0, 0, 0), w(0, 0, 0);
    
    // imu测量值 惯性系下
    double t = msg_imu->header.stamp.toSec();
    a(0) = msg_imu->linear_acceleration.x;
    a(1) = msg_imu->linear_acceleration.y;
    a(2) = msg_imu->linear_acceleration.z;
    w(0) = msg_imu->angular_velocity.x;
    w(1) = msg_imu->angular_velocity.y;
    w(2) = msg_imu->angular_velocity.z;
    
    // 加速度计求姿态直接用测量值三角代换, 无需用实际值积分
    // imu真实值  除去重量，此时还未考虑scale, bias, noise的影响
    imuProcess(a, w, t);

   
    geometry_msgs::TransformStamped tf_imu;
    tf_imu.header.stamp = msg_imu->header.stamp;
    tf_imu.child_frame_id = "imu_q";
    tf_imu.header.frame_id = "map";
    
    tf_imu.transform.rotation.x =  msg_imu->orientation.x;
    tf_imu.transform.rotation.y =  msg_imu->orientation.y;
    tf_imu.transform.rotation.z =  msg_imu->orientation.z;
    tf_imu.transform.rotation.w =  msg_imu->orientation.w;
    tf_imu.transform.translation.x = 0;
    tf_imu.transform.translation.y = 1;
    tf_imu.transform.translation.z = 0.5;
    static_broadcaster_->sendTransform(tf_imu);

    static geometry_msgs::Quaternion q_init = msg_imu->orientation;
    geometry_msgs::Quaternion q_cur = msg_imu->orientation;

    // tf::Quaternion _q;
    // tf::quaternionMsgToTF(q_init, _q);
    // double x,y,z;
    // tf::Matrix3x3(_q).setRPY(x,y,z);

    tf2::Quaternion q1, q2;
    double r,p,y, r1, p1, y1;
    tf2::fromMsg(q_init, q1);   // 初始值
    tf2::fromMsg(q_cur, q2);
    tf2::Matrix3x3(q1).getRPY(r,p,y);
    tf2::Matrix3x3(q2).getRPY(r1,p1,y1);

    
    double r2, p2, y2;
    tf2::Matrix3x3(q1).getRPY(r2, p2, y2);  // 把初始值赋给r2, p2, y2
    double dt1 = t - t0_;
    
    if(first_flag)
    {
        t0_ = t; // 更新时间
        first_flag = false;
    }
    
    r2 += msg_imu->angular_velocity.x * dt1;
    p2 += msg_imu->angular_velocity.y * dt1;
    y2 += msg_imu->angular_velocity.z * dt1;
   

    std::cout << std::fixed << std::setprecision(3) 
        << "q_init:    \t\t" << r * 180. / M_PI << "\t"
        << p * 180. / M_PI << "\t"
        << y * 180. / M_PI << std::endl;

    std::cout << std::fixed << std::setprecision(3) 
        << "q_current:  \t\t" << r1 * 180. / M_PI << "\t"
        << p1 * 180. / M_PI << "\t"
        << y1 * 180. / M_PI << std::endl;

    std::cout << std::fixed << std::setprecision(3) 
        << "integrate_w:\t\t" << r2 * 180. / M_PI << "\t"
        << p2 * 180. / M_PI << "\t"
        << y2 * 180. / M_PI << std::endl;
    

}

void TF2Node::imuProcess(Eigen::Vector3d &a, Eigen::Vector3d &w,const double &time)
{   
    double roll, pitch, yaw;
    double dt;
    current_time_ = time;
    // 1----------加速度算姿态角roll pitch-----------
    a.normalize();  // 归一化对比例计算结果无影响
    // tan2对象限敏感，根据两个参数判断它是属于哪个象限并给出对应的角度值，值域范围 [-pi, pi]；
    // atan对象限不敏感，值域范围为 [-pi/2, pi/2]。
    roll = atan2(a.y(), a.z());
    pitch = -atan2(a.x(),sqrt(a.y()*a.y() + a.z()*a.z())); // 正负号根据imu的四元素确定
    yaw = 0.;
    // ----------结束加速度算姿态角-----------

    euler_a_ << roll, pitch, yaw;
    std::cout << "acceleration compute:   " << (euler_a_ * 180 / M_PI).transpose() << std::endl;

    // 2----------陀螺仪算姿态角-----------
    if(first_imu_)
    {   
        first_imu_ = false;
        w0_ << 0, 0, 0;   // 让陀螺仪初始状态为0, 和右手定则重合
        euler_w0_ = euler_a_;
        last_time_ = time;

        q0_ = Eigen::Quaterniond::Identity();
    }
    dt = current_time_ - last_time_;
    // 角速度w积分时候转成四元素, 四元素单位化后可以代替旋转角积分得大于2π的角度
    Eigen::Quaterniond qw(1, 0.5 * w.x() * dt, 0.5 * w.y() * dt, 0.5 * w.y() * dt);
    
    // qw.normalize();
    // 四元素对时间积分
    q_w_ = qw * q0_;
    // 更新q0的值为下一次积分做准备
    q0_ = q_w_.normalized();
    // ---------结束陀螺仪算姿态角-----------

    euler_w_ = q_w_.toRotationMatrix().eulerAngles(2, 1, 0);
    Eigen::Vector3d e(euler_w_(2), euler_w_(1), euler_w_(0));
    
    std::cout << "angular velocity compute: " <<  (e * 180 / M_PI).transpose() << std::endl;
    std::cout << "--------------\n" << std::endl;

    geometry_msgs::TransformStamped tf_a;
    tf_a.header.stamp = ros::Time::now();
    tf_a.child_frame_id = "imu_acc";
    tf_a.header.frame_id = "map";

    tf2::Quaternion q_a;
    q_a.setRPY(euler_a_.x(), euler_a_.y(), euler_a_.z());
    tf_a.transform.rotation.x = q_a.x();
    tf_a.transform.rotation.y = q_a.y();
    tf_a.transform.rotation.z = q_a.z();
    tf_a.transform.rotation.w = q_a.w();
    tf_a.transform.translation.x = 0;
    tf_a.transform.translation.y = 0.5;
    tf_a.transform.translation.z = 0.5;

    static_broadcaster_->sendTransform(tf_a);

    geometry_msgs::TransformStamped tf_w;
    tf_w.header.stamp = ros::Time::now();
    tf_w.child_frame_id = "imu_angle";
    tf_w.header.frame_id = "map";

    q_a.setRPY(euler_w_.x(), euler_w_.y(), euler_w_.z());
    tf_w.transform.rotation.x = q_a.x();
    tf_w.transform.rotation.y = q_a.y();
    tf_w.transform.rotation.z = q_a.z();
    tf_w.transform.rotation.w = q_a.w();
    tf_w.transform.translation.x = 0;
    tf_w.transform.translation.y = 1.5;
    tf_w.transform.translation.z = 0.5;

    static_broadcaster_->sendTransform(tf_w);

}








int main(int argc, char** argv)
{   
    ros::init(argc, argv, "tf2_node");        
    TF2Node tf2node;
    ros::spin();
    return 0;
}